1. Field of the Invention
The present invention relates to a trajectory planning method, a trajectory planning system and a trajectory planning and control system in which a sequence of states of an object up to the goal state is generated.
2. Background Art
For example, assume that goal states of arms of a robot are realized by controlling torques around joints of the arms based on a trajectory planning. When positions and angles alone of the arms are handled in the trajectory planning, torques around the joints cannot be controlled. In order to control torques around the joints, a trajectory planning of a state space (phase space) which includes angles and angular velocities of the arms has to be generated.
As a method for handling state transitions in the phase space, there is a method using Rapidly-Exploring Random Tree (RET) (S. M. LaValle and J. J. Kuffner: “Randomized Kinodynamic Planning,” Journal of Robotics Research, Vol. 20, No. 5, pp. 378-400, (2001)). However, since RRT is a stochastic method, minimum-cost motion cannot necessarily be realized. Further, there is a method which has been proposed by Tazaki and in which quantization of the state space is utilized (Y. Tazaki and J. Imura: “Approximately Bisimilar Discrete Abstractions of Nonlinear Systems Using Variable-resolution Quantizers,” American Control Conference, pp. 1015-1020, (2010)). However, this method does not handle transitions in a continuous manner and the assumption of high linearity is required. Accordingly, applications of the method are limited to certain types of machines alone.
Under the situation, a motion planning method which uses division of the phase space and merging of branches of the search tree and by which quasi-minimum-cost motion can be planned through continuous search has been proposed by some of the inventors (C. H. Kim, H. Tsujino and S. Sugano: “Rapid Short-Time Path Planning for Phase Space,” Journal of Robotics and Mechatronics, Vol. 23, No. 2, (2011)). By the method, quasi-minimum-cost motion from a given initial state to a set of goal states under constraints can be planned in a short time period through continuous search. However, in this method, the phase space is divided uniformly along each axis and a size of cell is determined individually by the designer. Further, an effective method of division by which motion costs are minimized has not been realized.
Thus, a trajectory planning method, a trajectory planning system and a trajectory planning and control system by which the state space can be divided such that motion costs are minimized and quasi-minimum-cost motion can be planned with a higher accuracy have not been developed.
Accordingly, there is a need for a trajectory planning method, a trajectory planning system and a trajectory planning and control system by which the state space can be divided such that motion costs are minimized and quasi-minimum-cost motion can be planned with a higher accuracy.